import Joint from "./joint"
import CONSTANT from '../../constant';
import RotationalLimitMotor from "./rotational-limit-motor";
import Vec3 from "../../common/vec3";
import Method from "../../common/method";

export default class RagdollJoint extends Joint{
    constructor(config) {
        super(config,CONSTANT.JOINT_TYPE_RAG_DOLL);
        Method.copyElements(config.localTwistAxis1.elements,this.localBasis1,0,0,3);
        Method.copyElements(config.localSwingAxis1.elements,this.localBasis1,0,3,3);
        Method.copyElements(config.localTwistAxis2.elements,this.localBasis2,0,0,3);
        this.buildLocalBasesFromXY1X2();
        this._twistSd = config.twistSpringDamper.clone();
        this._twistLm = config.twistLimitMotor.clone();
        this._swingSd = config.swingSpringDamper.clone();
        this._maxSwingAngle1 = config.maxSwingAngle1;
        this._maxSwingAngle2 = config.maxSwingAngle2;
        if(this._maxSwingAngle1 < CONSTANT.SETTING_MIN_RAG_DOLL_MAX_SWING_ANGLE) {
            this._maxSwingAngle1 = CONSTANT.SETTING_MIN_RAG_DOLL_MAX_SWING_ANGLE;
        }
        if(this._maxSwingAngle2 < CONSTANT.SETTING_MIN_RAG_DOLL_MAX_SWING_ANGLE) {
            this._maxSwingAngle2 = CONSTANT.SETTING_MIN_RAG_DOLL_MAX_SWING_ANGLE;
        }
        this.dummySwingLm = new RotationalLimitMotor();
        this.dummySwingLm.lowerLimit = -1;
        this.dummySwingLm.upperLimit = 0;
        this._swingAngle = 0;
        this._twistAngle = 0;
        this.swingError = 0;
        this.swingAxis=new Float64Array(3);
        this.twistAxis=new Float64Array(3);
        this.linearError=new Float64Array(3);
        this._tm1=new Float64Array(9);
        this._tv1=new Float64Array(4);
    }
    getInfo(info,timeStep,isPositionPart) {
        let erp = this.getErp(timeStep,isPositionPart);
        let le=this.linearError;
        let ra1=this.relativeAnchor1,ra2=this.relativeAnchor2;
        let bs1=this.basis1,bs2=this.basis2;
        let sa=this.swingAxis,ta=this.twistAxis;
        let linearRhsX = le[0] * erp,linearRhsY = le[1] * erp,linearRhsZ = le[2] * erp;
        let c100 = 0,c101 = ra1[2],c102 = -ra1[1];
        let c110 = -ra1[2],c111 = 0,c112 = ra1[0];
        let c120 = ra1[1],c121 = -ra1[0],c122 = 0;
        let c200 = 0,c201 = ra2[2],c202 = -ra2[1];
        let c210 = -ra2[2],c211 = 0,c212 = ra2[0];
        let c220 = ra2[1],c221 = -ra2[0],c222 = 0;
        let swingMass = this.computeEffectiveInertiaMoment(sa[0],sa[1],sa[2]);
        let twistMass = this.computeEffectiveInertiaMoment(bs2[0],bs2[1],bs2[2]);
        let impulse = this._impulses[0];
        let row = info.rows[info.numRows++];
        this._resetRow(row,impulse);
        row.rhs = linearRhsX;
        row.cfm = 0;
        row.minImpulse = -1e65536;
        row.maxImpulse = 1e65536;
        Method.setJacobianElements(row.jacobian.elements,1,0,0,1,0,0,c100,c101,c102,c200,c201,c202);
        let impulse1 = this._impulses[1];
        let row1 = info.rows[info.numRows++];
        this._resetRow(row1,impulse1);
        row1.rhs = linearRhsY;
        row1.cfm = 0;
        row1.minImpulse = -1e65536;
        row1.maxImpulse = 1e65536;
        Method.setJacobianElements(row1.jacobian.elements,0,1,0,0,1,0,c110,c111,c112,c210,c211,c212);
        let impulse2 = this._impulses[2];
        let row2 = info.rows[info.numRows++];
        this._resetRow(row2,impulse2);
        row2.rhs = linearRhsZ;
        row2.cfm = 0;
        row2.minImpulse = -1e65536;
        row2.maxImpulse = 1e65536;
        Method.setJacobianElements(row2.jacobian.elements,0,0,1,0,0,1,c120,c121,c122,c220,c221,c222);
        if(this.swingError > 0 && (this._swingSd.frequency <= 0 || !isPositionPart)) {
            let impulse = this._impulses[3];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowAngular(row,this.swingError,this.dummySwingLm,swingMass,this._swingSd,timeStep,isPositionPart);
            let j = row.jacobian.elements;
            j[6] = sa[0];j[7] = sa[1];j[8] = sa[2];
            j[9] = sa[0];j[10] = sa[1];j[11] = sa[2];
        }
        if(this._twistSd.frequency <= 0 || !isPositionPart) {
            let impulse = this._impulses[4];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowAngular(row,this._twistAngle,this._twistLm,twistMass,this._twistSd,timeStep,isPositionPart);
            let j = row.jacobian.elements;
            j[6] = ta[0];j[7] = ta[1];j[8] = ta[2];
            j[9] = ta[0];j[10] = ta[1];j[11] = ta[2];
        }
    }
    _syncAnchors() {
        super._syncAnchors();
        let sa=this.swingAxis,ta=this.twistAxis;
        let le=this.linearError;
        let bs1=this.basis1,bs2=this.basis2;
        let tv=this._tv,tm=this._tm,tm1=this._tm1,tv1=this._tv1;
        Method.setRotFromTwoVec3(bs1[0],bs1[1],bs1[2],bs2[0],bs2[1],bs2[2],tv,tm);
        Method.transposeM33(bs1,tm1);
        this._swingAngle = (tv[3] <= -1 ? 3.14159265358979 : tv[3] >= 1 ? 0 : Math.acos(tv[3])) * 2;
        Method.setXYZ(tv1,bs2[3],bs2[4],bs2[5]);
        Method.inverseRotateVec3(tv1,tm);
        this._twistAngle = Math.atan2(bs1[6] * tv1[0] + bs1[7] * tv1[1] + bs1[8] * tv1[2],bs1[3] * tv1[0] + bs1[4] * tv1[1] + bs1[5] * tv1[2]);
        Method.addArray(bs1,bs2,ta,0,0,0,3);
        Method.normalize(ta,0,3);
        Method.normalize(tv,0,3,this._swingAngle);

        Method.inverseRotateVec3(tv,tm1);
        let x1 = tv[1];
        let y1 = tv[2];
        let a = this._maxSwingAngle1;
        let b = this._maxSwingAngle2;
        let invA2 = 1 / (a * a);
        let invB2 = 1 / (b * b);
        let w1 = x1 * x1 * invA2 + y1 * y1 * invB2;
        if(w1 === 0) {
            Method.fillValue(sa,0,2,0);
            this.swingError = 0;
        } else {
            let t = Math.sqrt(1 / w1);
            let x0 = x1 * t;
            let y0 = y1 * t;
            let nx = x0 * invA2;
            let ny = y0 * invB2;
            let invLen = 1 / Math.sqrt(nx * nx + ny * ny);
            nx *= invLen;
            ny *= invLen;
            let depth = (x1 - x0) * nx + (y1 - y0) * ny;
            if(depth > 0) {
                this.swingError = depth;
                Method.setXYZ(sa,0,nx,ny);
                Method.rotateVec3(sa,tm1);
                Method.rotateVec3(sa,tm);
            } else {
                this.swingError = 0;
            }
        }
        Method.subArray(this.anchor2,this.anchor1,le,0,0,0,3);
    }
    _getVelocitySolverInfo(timeStep,info) {
        super._getVelocitySolverInfo(timeStep,info);
        this.getInfo(info,timeStep,false);
    }
    _getPositionSolverInfo(info) {
        super._getPositionSolverInfo(info);
        this.getInfo(info,null,true);
    }
    getAxis1() {
        return new Vec3(this.basis1[0],this.basis1[1],this.basis1[2]);
    }
    getAxis2() {
        return new Vec3(this.basis2[0],this.basis2[1],this.basis2[2]);
    }
    getAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.basis1[0];es[1] = this.basis1[1];es[2] = this.basis1[2];
    }
    getAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.basis2[0];es[1] = this.basis2[1];es[2] = this.basis2[2];
    }
    getLocalAxis1() {
        return new Vec3(this.localBasis1[0],this.localBasis1[1],this.localBasis1[2]);
    }
    getLocalAxis2() {
        return new Vec3(this.localBasis2[0],this.localBasis2[1],this.localBasis2[2]);
    }
    getLocalAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis1[0];es[1] = this.localBasis1[1];es[2] = this.localBasis1[2];
    }
    getLocalAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis2[0];es[1] = this.localBasis2[1];es[2] = this.localBasis2[2];
    }
    getTwistSpringDamper() {
        return this._twistSd;
    }
    getTwistLimitMotor() {
        return this._twistLm;
    }
    getSwingSpringDamper() {
        return this._swingSd;
    }
    getSwingAxis() {
        return new Vec3(this.swingAxis[0],this.swingAxis[1],this.swingAxis[2]);
    }
    getSwingAxisTo(axis) {
        let es=axis.elements;
        es[0] = this.swingAxis[0];es[1] = this.swingAxis[1];es[2] = this.swingAxis[2];
    }
    getSwingAngle() {
        return this._swingAngle;
    }
    getTwistAngle() {
        return this._twistAngle;
    }
}